/**
 * @file full_graph_mapping.cc
 * @author DustinKsi (dustinksi@126.com)
 * @brief
 * @version 0.1
 * @date 2019-12-23
 *
 * @copyright Copyright (c) 2019
 *
 */

#include "venus/mapping/full_graph_mapping/full_graph_mapping.h"

namespace VEnus {
namespace Mapping {

FullGraphMapping::~FullGraphMapping() { CLASS_LOG(FullGraphMapping) << "released!"; }

bool FullGraphMapping::Init() {
    return true;
}

bool FullGraphMapping::SetInitialPose(VEnus::Sensor::RobotPose &pose) {return false;}

bool FullGraphMapping::InsertFeatureList(VEnus::Sensor::Feature2DList &feature_list,
                                         VEnus::Sensor::AdjacencyList &adjacency_list,
                                         VEnus::Sensor::RobotPose &pose) {
  if (feature_list.feature_2d_with_ids_size() < 4) {
    D_WARN << "[REJECT] feature points' size not enough for mapping.";
    return false;
  }

  unordered_map<int, pair<double, double> > _l_fpts_list;
  int fake_id = 0;
  for (VEnus::Sensor::Feature2DWithID fpt : feature_list.feature_2d_with_ids()) {
    _l_fpts_list[fake_id] = (make_pair(fpt.x(), fpt.y()));

    fake_id++;
  }
  feature_list.clear_feature_2d_with_ids();

  // Initial _g_fpts_list
  if (_g_fpts_list.empty()) {
    for (auto pr : _l_fpts_list) {
      _g_fpts_list[_g_feature_id_cnt] = pr.second;

      auto fpt = feature_list.add_feature_2d_with_ids();
      fpt->set_x(pr.second.first);
      fpt->set_y(pr.second.second);
      fpt->set_id(_g_feature_id_cnt);

      _g_feature_id_cnt++;
    }

    return true;
  }

  // got part of feature map, start matching... at least [4] feature input
  // match fpts to _fpts_list

  return true;
}
/**
 * @brief
 *
 * @param edges_list input edges list for hit check
 * @param hit_edges_result edges got hit [local_id pair] -> [global_id pair]
 * @param nohit_edges_index edges not got hit's index
 * @return true
 * @return false
 */
// bool MatchEdgesToGlobal(map<pair<int, int>, double> &edges_list,
//                         vector<pair<pair<int, int>, pair<int, int> > > &hit_edges_result,
//                         vector<int> &nohit_edges_index) {
//   nohit_edges_index.clear();

//   unordered_set<int> visited;
//   int fpts_sz = _g_fpts_list.size();
//   for (int i = 0; i < fpts_sz; i++) {
//     if (visited.find(i) != visited.end()) {
//       continue;
//     }
//     visited.insert(i);
//     for (int nxt : graph[i]) {
//       visited.insert(nxt);
//       if (visited.find(nxt) != visited.end()) {
//         continue;
//       }
//       auto fpts_pr = make_pair(i, nxt);
//       if (_g_edges_list.find(fpts_pr) != _g_edges_list.end()) {
//         GotHit(edges_list, fpts_pr, hit_edges_list);
//       }
//     }
//   }
//   return true;
// }

/**
 * @brief
 *
 * @param fpts_list
 * @param distance_dict
 * @param edges_heap
 * @param graph
 * @return true
 * @return false
 */
bool FullGraphMapping::GenerateListGraphAndHeap(
    unordered_map<int, pair<double, double> > &fpts_list,
    map<pair<int, int>, double> &distance_dict, priority_queue<EdgeWithLen> &edges_heap,
    unordered_map<int, int> &graph) {
  int sz = fpts_list.size();
  double distance_square_ceil_threshold = pow(_options.distance_ceil_threshold(), 2);
  for (int i = 0; i < sz - 1; i++) {
    for (int j = i + 1; j < sz; j++) {
      auto p1 = fpts_list[i];
      auto p2 = fpts_list[j];
      double distance_square = pow(p1.first - p2.first, 2) + pow(p1.second - p2.second, 2);
      if (distance_square > distance_square_ceil_threshold) {
        continue;
      }
      double length = sqrt(distance_square);
      edges_heap.push(EdgeWithLen(i, j, length));

      distance_dict[make_pair(i, j)] = length;
      distance_dict[make_pair(j, i)] = length;

      graph[i] = j;
      graph[j] = i;
    }
  }
  return true;
}

bool FullGraphMapping::UpdateEdgeList(unordered_map<int, pair<double, double> > &fpts_list) {
    return false;
}

/**
 * @brief
 *
 * @param fpts_list local
 * @param distance_dict local
 * @param edges_heap local
 * @param graph local
 * @return true
 * @return false
 */
bool FullGraphMapping::ExtractKeyEdge(unordered_map<int, pair<double, double> > &fpts_list,
                                      unordered_map<int, double> &score_dict) {
  map<pair<int, int>, double> distance_dict;
  priority_queue<EdgeWithLen> edges_heap;
  unordered_map<int, int> graph;
  GenerateListGraphAndHeap(fpts_list, distance_dict, edges_heap, graph);

  vector<EdgeWithLen> global_edges_sorted;
  while (!_g_edges_heap.empty()) {
    global_edges_sorted.push_back(_g_edges_heap.top());
    _g_edges_heap.pop();
  }

  while (!edges_heap.empty()) {
    auto target_len = edges_heap.top().edge_length;
    for (auto fedge : global_edges_sorted) {
      double dlt_distance = fabs(fedge.edge_length - target_len);
      if (dlt_distance < 0.1) {
        D_INFO << fedge.greator_id << "-" << fedge.less_id << ": " << dlt_distance
               << " has been scored";
        score_dict[fedge.greator_id] += dlt_distance;
        score_dict[fedge.less_id] += dlt_distance;
      }
      // if(dlt_distance > 1.0)
      // {
      //   break;
      // }
    }
    edges_heap.pop();
  }
  return true;
}

void FullGraphMapping::GenerateScoreGraphImage(unordered_map<int, pair<double, double> > &fpts_list,
                                               unordered_map<int, double> score_dict,
                                               cv::Mat &graph_image) {
  cv::Size2i graph_size = graph_image.size();
  double bias_x = 120., bias_y = 20.;
  double scale_x, scale_y;

  double max_x = INT_MIN, min_x = INT_MAX, max_y = INT_MIN, min_y = INT_MAX;
  for (auto fpt : fpts_list) {
    auto pt = fpt.second;
    if (pt.first < min_x) {
      min_x = pt.first;
    }
    if (pt.first > max_x) {
      max_x = pt.first;
    }
    if (pt.second < min_y) {
      min_y = pt.second;
    }
    if (pt.second > max_y) {
      max_y = pt.second;
    }
  }

  scale_x = (graph_size.width - 2 * bias_x) / (max_x - min_x);
  scale_y = (graph_size.height - 2 * bias_y) / (max_y - min_y);
  double scale;
  if (scale_x < scale_y) {
    scale = scale_x;
    bias_y = (graph_size.height - scale * (max_y - min_y)) / 2.0f;
  } else {
    scale = scale_y;
    bias_x = (graph_size.width - scale * (max_x - min_x)) / 2.0f;
  }

  for (auto pr : score_dict) {
    auto pt = fpts_list[pr.first];
    double score = pr.second;
    cv::Point2d pfpt((pt.first - min_x) * scale + bias_x,
                     graph_size.height - (pt.second - min_y) * scale - bias_y);

    stringstream ss;
    ss << "score: " << std::setprecision(3) << score;
    pfpt.x += 10;
    pfpt.y -= 20;
    cv::putText(graph_image, ss.str(), pfpt, cv::FONT_HERSHEY_COMPLEX, 0.5, CV_RGB(0, 127, 205), 1);
  }
}

void FullGraphMapping::GenerateGraphImage(unordered_map<int, pair<double, double> > &fpts_list,
                                          map<pair<int, int>, double> &distance_dict,
                                          cv::Mat &graph_image) {
  cv::Size2i graph_size = graph_image.size();
  double bias_x = 120., bias_y = 20.;
  double scale_x, scale_y;

  double max_x = INT_MIN, min_x = INT_MAX, max_y = INT_MIN, min_y = INT_MAX;
  for (auto fpt : fpts_list) {
    auto pt = fpt.second;
    if (pt.first < min_x) {
      min_x = pt.first;
    }
    if (pt.first > max_x) {
      max_x = pt.first;
    }
    if (pt.second < min_y) {
      min_y = pt.second;
    }
    if (pt.second > max_y) {
      max_y = pt.second;
    }
  }

  scale_x = (graph_size.width - 2 * bias_x) / (max_x - min_x);
  scale_y = (graph_size.height - 2 * bias_y) / (max_y - min_y);
  double scale;
  if (scale_x < scale_y) {
    scale = scale_x;
    bias_y = (graph_size.height - scale * (max_y - min_y)) / 2.0f;
  } else {
    scale = scale_y;
    bias_x = (graph_size.width - scale * (max_x - min_x)) / 2.0f;
  }

  for (auto eg : distance_dict) {
    auto pt1 = fpts_list[eg.first.first];
    auto pt2 = fpts_list[eg.first.second];
    cv::Point2d fpt1((pt1.first - min_x) * scale + bias_x,
                     graph_size.height - (pt1.second - min_y) * scale - bias_y);
    cv::Point2d fpt2((pt2.first - min_x) * scale + bias_x,
                     graph_size.height - (pt2.second - min_y) * scale - bias_y);

    cv::line(graph_image, fpt1, fpt2, CV_RGB(205, 127, 0), 1);
  }

  for (auto fpt : fpts_list) {
    auto pt = fpt.second;
    cv::Point2d pfpt((pt.first - min_x) * scale + bias_x,
                     graph_size.height - (pt.second - min_y) * scale - bias_y);
    cv::circle(graph_image, pfpt, 3, CV_RGB(0, 127, 205), 1);
    stringstream ss;
    ss << std::setprecision(2) << pt.first << ", " << pt.second;
    pfpt.x += 10;
    cv::putText(graph_image, ss.str(), pfpt, cv::FONT_HERSHEY_COMPLEX, 0.5, CV_RGB(0, 127, 205), 1);
  }
}

FullGraphMapping::FullGraphMapping() { CLASS_LOG(FullGraphMapping) << "built!!"; }

}  // namespace Mapping
}  // namespace VEnus
